classdef Policy < handle
    properties

        %% 模型参数
        init                    % 是否初始化
        globalMap               % 全局地图
        startPos                % 小车起始位置
        startH                  % 小车起始朝向
        endPos                  % 小车目标位置
        satLevel                % 小车速度与转角改变的限制范围
        carWidth                % 小车宽度
        carLength               % 小车长度
        lastU                   % 上一次的速度
        lastV                   % 上一次的转角

        %% 混合A*搜索
        % 自适应参数
        obstTolerance           % 障碍物容忍度
        maxSeconds              % 一次搜索的最大搜索时间
        % 结果变量
        x                       % 小车路径x
        y                       % 小车路径y
        theta                   % 小车路径朝向
        D                       % 小车路径距离
        delta                   % 小车路径转角
        absU                    % 小车路径速度绝对值
        % 重新规划标志
        replan                  % 是否重新规划路径

        %% 路径跟踪
        traceId                 % 跟踪索引
        analyzer                % 轨迹分析器
        kp                      % 纯P跟踪参数
        forwardNumber           % 前瞻点数

    end

    methods
        
        function self = Policy()
            self.init = 0;
            self.lastU = 0;
            self.lastV = 0;
            self.obstTolerance = 0.2;
            self.maxSeconds = 30*60;
            self.x = [];
            self.y = [];
            self.theta = [];
            self.D = [];
            self.delta = [];
            self.traceId = 0;
            self.kp = 1;
            self.forwardNumber = 3;
        end

        function action=action(self,observation)

            %% 获取核心信息
            t = observation.t;            	%#ok<*NASGU> % 当前所用时间
            agent = observation.agent;		% 当前小车对象

            %% 初始化信息
            if self.init==0
                self.globalMap = observation.map;          % 全局地图，0可通行，1障碍物
                self.startPos = [observation.startPos.x, ...
                    observation.startPos.y];                % 小车起始位置
                self.startH = observation.startPos.heading; % 小车起始朝向
                self.endPos = [observation.endPos.x, ...
                    observation.endPos.y];                  % 小车目标位置
                self.satLevel = agent.satLevel;             % 小车前进速度与转动速度的限制范围
                self.carWidth = agent.ht;                   % 小车宽度
                self.carLength = agent.w;                   % 小车长度
                self.PlanPath();                            % 混合A*算法规划路径
                self.init=1;
            end

            %% 获取当前信息
            % scanMap = observation.scanMap	  % 当前雷达探测器探测到的信息  
            curX = agent.x;            	    % 当前小车位置x
            curY = agent.y;            	    % 当前小车位置y
            curTheta = agent.h;       	    % 当前小车朝向，单位：弧度
            collide = observation.collide;  % 当前是否发生碰撞

            %% 重新规划路径
            if self.replan==1
                self.startPos = [curX,curY];% 小车起始位置
                self.startH = curTheta;     % 小车起始朝向
                self.PlanPath();            % 混合A*算法规划路径
            end
            
            %% 路径跟踪
            if collide
                % 返回上一次的位置
                u = -self.lastU;
                v = 0;

                % 使能路径跟踪索引，减小障碍物距离限制
                self.replan = 1;
                self.obstTolerance = self.obstTolerance - 0.1;
            else
                [direction,thetaError,nextId] = ...
                    self.analyzer.AnalyzePath(curX,curY,curTheta,self.traceId);
                if direction > 0
                    u = self.absU;
                else
                    u = -self.absU;
                end
                v = self.kp*thetaError;
                disp(['traceId:',num2str(self.traceId),' u:',num2str(u),' v:',num2str(v)]);
                self.traceId = nextId;
            end
            
            %% 导出动作，保存动作
            action = [u, v];                % 小车前进速度u，小车转动速度v
            self.lastU = u;
            self.lastV = v;
        end

        function PlanPath(self)
            % 混合A*算法搜索路径
            [self.x,self.y,self.theta,self.D,self.delta,~,steeringTimes] = ...
                SearchPath(self.obstTolerance,self.globalMap,[self.startPos,self.startH], ...
                self.endPos,self.carWidth,self.carLength,self.satLevel,0,self.maxSeconds);

            % 初始化轨迹分析器
            self.analyzer = TrajectoryAnalyzer(self.x, ...
                self.y,self.theta,self.D,self.delta, ...
                self.forwardNumber);

            % 初始化速度
            self.absU = self.satLevel(1)/steeringTimes;

            % 重置路径跟踪索引
            self.replan = 0;
            self.traceId = 0;
        end
        
    end
end